import os
import json
from glob import glob
from scipy.spatial.transform import Rotation as R

def create_incremental_folder(current_dir):
    """
    在 current_dir 目录下创建一个递增命名的子文件夹，
    文件夹名从 0 开始递增，如果文件夹已存在则递增数字。
    """
    # 设置初始文件夹名为 0
    folder_name = '0'
    
    # 构建文件夹路径
    folder_path = os.path.join(current_dir, folder_name)
    
    # 检查文件夹是否已经存在，如果存在，递增数字直到找到一个不存在的文件夹名
    counter = 0
    while os.path.exists(folder_path):
        counter += 1
        folder_name = str(counter)
        folder_path = os.path.join(current_dir, folder_name)
    
    # 创建文件夹
    os.makedirs(folder_path)
    print(f"Folder '{folder_name}' created at {folder_path}")
    return folder_path


def save_args_to_json(args, current_dir, filename="params.json"):
    """
    将命令行参数保存到 JSON 文件。
    
    参数：
    args (Namespace): 命令行参数对象
    filename (str): 保存的 JSON 文件名
    """
    # 将命令行参数转化为字典
    args_dict = vars(args)

    file_path = os.path.join(current_dir, filename)
    
    # 将字典写入 JSON 文件
    with open(file_path, 'w') as json_file:
        json.dump(args_dict, json_file, indent=4)
    
    print(f"Parameters saved to {file_path}")


def find_files_in_folder(folder_paths, pattern):
    found_files = []
    for folder_path in folder_paths:
        if os.path.exists(folder_path):  # 检查文件夹是否存在
            # 使用glob查找符合模式的文件
            matched_files = glob(os.path.join(folder_path, pattern))
            found_files.extend(matched_files)
        else:
            print(f"Folder not found: {folder_path}")
    return found_files

def find_file_in_folder(folder_paths, pattern):

    if os.path.exists(folder_paths):  # 检查文件夹是否存在
        # 使用glob查找符合模式的文件
        matched_files = glob(os.path.join(folder_paths, pattern))
        if len(matched_files) > 0:
            print(matched_files)
            return matched_files[0]
        else:
            print(f"Folder not found: {folder_paths}")
            return None
    else:
        print(f"Folder not found: {folder_paths}")
        return None
    

def get_paramter_from_json(json_path, param_name):
    with open(json_path, 'r') as json_file:
        params = json.load(json_file)
        param = params.get(param_name)
    return param



def quaternion_to_euler(q_x, q_y, q_z, q_w):
    """
    Convert quaternion to Euler angles (roll, pitch, yaw).
    Parameters:
        q_x, q_y, q_z, q_w: Components of the quaternion.
    Returns:
        tuple: roll, pitch, yaw in radians.
    """
    r = R.from_quat([q_x, q_y, q_z, q_w])
    return r.as_euler('zyx', degrees=False)  # Return ypr

def find_nearest(data, timestamps):
    """
    Find the nearest data rows in 'data' based on the given timestamps.
    
    Parameters:
        data (np.ndarray): The array to search in, shape (N, 5), where column 0 is the timestamp.
        timestamps (np.ndarray): The array of timestamps to align with, shape (M,).
    
    Returns:
        np.ndarray: The aligned data from 'data', shape (M, 5).
    """
    # Extract the timestamps from the input data
    data_timestamps = data[:, 0]
    
    # Find the nearest indices for each timestamp in 'timestamps'
    nearest_indices = np.searchsorted(data_timestamps, timestamps, side='left')
    
    # Handle edge cases where indices are out of bounds
    nearest_indices = np.clip(nearest_indices, 0, len(data_timestamps) - 1)
    
    # Return the rows from 'data' corresponding to the nearest timestamps
    return data[nearest_indices]